package com.grt192.benchtest.mechanism;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Mechanism;

/**
 *
 * @author anand
 */
public class BenchFinaleMechanism extends Mechanism {

    private GRTJaguar leftMotor;
    private GRTJaguar rightMotor;
    private boolean debug;

    public BenchFinaleMechanism(int left, int right, boolean debug) {
        leftMotor = new GRTJaguar(left);
        leftMotor.start();
        rightMotor = new GRTJaguar(right);
        rightMotor.start();
        this.debug = debug;
    }

    public void extend() {
        extend(1.0);
    }

    public void extend(double speed) {
        if (debug) {
            System.out.println("F F");
        }
        leftMotor.enqueueCommand(speed);
        rightMotor.enqueueCommand(speed);
    }

    public void retract(double speed) {
        if (debug) {
            System.out.println("F R");
        }
        leftMotor.enqueueCommand(-speed);
        rightMotor.enqueueCommand(-speed);
    }

    public void retract() {
        retract(1.0);
    }

    public void stop() {
        if (debug) {
            System.out.println("F S");
        }
        leftMotor.enqueueCommand(0);
        rightMotor.enqueueCommand(0);
    }
}
